import math
import threading
import robomaster
from host import host
from udp2 import udp_client5, udp_client6


class Flying:
    def __init__(self, tl_drone):
        """
        初始化飞行类实例
        :param tl_drone: 无人机对象
        """
        self.drone: robomaster.robot.Drone = tl_drone
        self.flight: robomaster.flight.Flight = self.drone.flight
        self.yaw = 0    # 转向角，根据运动所得，不是通过传感器

    def forward_udp(self, distance):
        """
        :param distance:
        """
        data_dict = {'x': distance * math.cos(self.yaw / 360 * 2 * math.pi), 'y': distance * math.sin(self.yaw / 360 * 2 * math.pi) , 'z': 0, 'speed': 20}
        # 格式化待发送的数据
        data_str = 'x:{x},y:{y},z:{z},speed:{speed}'.format(**data_dict)
        # 将数据转换成字节串
        packet = bytes(data_str, encoding='utf-8')
        udp_client5.sendto(packet, (host, 20001))
        self.flight.forward(distance).wait_for_completed()

    def takeof_udp(self):
        self.flight.takeoff().wait_for_completed()

    def land_udp(self):
        self.flight.land().wait_for_completed()

    def up_udp(self, distance):
        self.flight.up(distance).wait_for_completed()

    def down_udp(self, distance):
        self.flight.down().wait_for_completed()

    def rotate_udp(self, angle):
        self.yaw = self.yaw + angle
        self.flight.rotate(angle).wait_for_completed()

    def go_udp(self, x, y, z, speed):
        data_dict = {'x': x * int(math.cos(self.yaw / 360 * 2 * math.pi)) + y * int(math.sin(self.yaw / 360 * 2 * math.pi)), 'y':  x * int(math.sin(self.yaw / 360 * 2 * math.pi)) + -1 * y * int(math.cos(self.yaw / 360 * 2 * math.pi)), 'z': z, 'speed': speed}
        data_str = 'x:{x},y:{y},z:{z},speed:{speed}'.format(**data_dict)
        print(x * int(math.cos(self.yaw / 360 * 2 * math.pi)))
        packet = bytes(data_str, encoding='utf-8')
        udp_client5.sendto(packet, (host, 20001))
        self.flight.go(x,y,z,speed).wait_for_completed()

    def rc_udp(self, a=0, b=0, c=0, d=0):
        data_dict = {'x': 0, 'y': 0, 'z': 0, 'speed_x': b * math.cos(self.yaw / 360 * 2 * math.pi),
                     'speed_y': b * math.sin(self.yaw / 360 * 2 * math.pi),
                     'speed_z': 0, 'speed': math.sqrt(a * a + b * b)}
        # 格式化待发送的数据
        data_str = 'x:{x},y:{y},z:{z},speed:{speed},speed_x:{speed_x},speed_y:{speed_y}'.format(**data_dict)
        # 将数据转换成字节串
        packet = bytes(data_str, encoding='utf-8')
        udp_client5.sendto(packet, (host, 20001))
        self.flight.rc(a=a, b=b, c=c, d=d)

    def stop_udp(self):
        data = 'ok'
        packet = bytes(data, encoding='utf-8')
        udp_client6.sendto(packet, (host, 2001))
        self.flight.stop()

    def curve_udp(self):
        self.flight.curve().wait_for_completed()